/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "ecatmastervrep.h"

#include "robotcontrol.h"
#include "powermanager.h"
#include "robotmanager.h"

#include <QHostAddress>
#include <QCoreApplication>
#include <QDebug>
#include <QString>
#include "vrepobject.h"
#include "vrepcontrol.h"
#include "GeneralDefine.h"
#include "messagelog.h"
#include "robotbase.h"
#include "ecatbusmanager.h"

extern RobotControl* robCtl;
extern PowerManager *p_powerManger;


EcatMasterVrep::EcatMasterVrep()
{
//    tcpVrep=NULL;
}

EcatMasterVrep::~EcatMasterVrep()
{
//    if(  tcpVrep!=NULL )
//    {
//        tcpVrep->close();
//    }
}


void EcatMasterVrep::masterInit()
{

//    pointProInterval =  50*1000 *1000/ cycle_ns ;
//    pointProInterval_counter = 0;
 //   v_encoder_t.resize( 0 );
    //sleep for robot initial
    sleep(3);
    robCtl->busManager->addVirtualDevices();

    int nodeReturn,aliasReturn;
    int returnKey=robCtl->busManager->checkRobotLogicalAxisMapEthercatAxis(nodeReturn,aliasReturn);
    if( 1==returnKey )
    {
        printf("MSG: scan bus: ethercat 设备映射 ok! \n");
    }
    else if( -2==returnKey )
    {
        printf("ERROR: 机器人轴别名在总线配置中没有找到 \n");
        robCtl->ecatmaster->addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 300, -1 ,aliasReturn);

    }
    else if( -100==returnKey )
    {
        printf("ERROR: 机器人轴别名busDev.xml中node与实际物理node不一致，请检查物理接线或配置! \n");
        robCtl->ecatmaster->addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 301, -1, aliasReturn,nodeReturn);
    }

}

void EcatMasterVrep::masterOp()
{
    if(false==isInitialedOk)
    {
        sleep(3);
        isInitialedOk=true;
        isSlavesOp = true;
        addMsg(ENUM_MSG_REMIND, "EcatMaster", "EcatMasterIgh" , 1002, -1 );
    }


    if(true==isInitialedOk)
    {
        updateDomainData(); //读写过程数据 (帧数据交互)
    }

//    //--------------------------------------

//    RobotBase* rob;
//    foreach( rob, robCtl->robotsMap )
//    {
//        if( !rob || !rob->isLinkOk ){
//            continue;
//        }
//        if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
//        {
//            //vrep没得选
//        }else{
//            //read vrep motor
//            VrepObject* vob=robCtl->vobMap.value( rob->robotInfo.robotId, NULL );
//            if( (p_powerManger!=NULL) && (vob!=NULL) ){
//                RobotEngine* robt = p_powerManger->robotManager->robotsMap.value( rob->robotInfo.robotId, NULL );
//                for( int i=0;i<rob->axis.size();i++ ){
//                    rob->axis.at(i)->actPosition = robt->threadMotionEngine ->mcControllerInterpreter->getVrepRadianToEncoder(  i, vob->getPos[i] );
//                    rob->axis.at(i)->encoderValue = rob->axis.at(i)->actPosition-rob->axis.at(i)->homeOffset;
//                    //read state
//                    if( (rob->axis.at(i)->controlWord & 0x0F) == 0x0b ){
//                        rob->axis.at(i)->stateWord= 561 ;
//                    }else if( (rob->axis.at(i)->controlWord & 0x0F) == 0x0F ) {
//                        rob->axis.at(i)->stateWord = 4663 ;
//                    }else  if( (rob->axis.at(i)->controlWord & 0x07) == 0x07 ){
//                        rob->axis.at(i)->stateWord= 0x0023 ;
//                    }else  {
//                        rob->axis.at(i)->stateWord= 561 ;
//                    }
//                }
//            }
//        }
//        if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
//        {
//            //vrep没得选
//        }else{
//            //read vrep io
//        }

//        // rob->test_demo();
//        for( int i=0;i<rob->axis.size();i++ )
//        {
//            rob->axis.at(i)->updateEthercatState();
//        }

//        if( rob->IO->siCount>1 )
//        {
//            rob->IO->ioSi[0]=true;
//        }
//        rob->checkError();
//        rob->updateEthercatIoData();
////        rob->checkHomeOffset();

//         rob->setServoData();
//        rob->test_demo();

//        if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
//        {
//            //vrep没得选
//        }
//        if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
//        {
//            //vrep没得选
//        }

//        if( robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_REAL )
//        {
//            if( rob->get_data_flag ){
//                //write vrep motor
//                VrepObject* vob=robCtl->vobMap.value( rob->robotInfo.robotId, NULL );
//                if( vob && p_powerManger ){
//                    RobotEngine* robt = p_powerManger->robotManager->robotsMap.value( rob->robotInfo.robotId, NULL );
//                    for( int i=0;i<rob->axis.size();i++ ){
//                        vob->setPos[i] = robt->threadMotionEngine->mcControllerInterpreter
//                                ->getVrepEncoderToRadian( i , rob->axis.at(i)->targetPosition+rob->axis.at(i)->homeOffset );

//                    }
//                    vob->setChanged=true;
//                }
//            }
//        }

//        if( robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_REAL )
//        {
//            //write vrep io
//        }

//    }

//#if D_IF_DEBUG
//    static int tCount=0;
//    tCount++;
//    if(tCount>10000000/cycle_us){
//        tCount=0;
//        qDebug()<<"==================================================";
//        for( int j=0;j<rob->axis.size();j++ )
//        {
//            printf("%d:MD:%d,ST:%d, ctl:%d,err:%d, encode:%d,%d,%d,%d, %d,%d\n",
//                   j,
//                   rob->axis.at(j)->modeOfOperation,
//                   rob->axis.at(j)->stateWord,
//                   rob->axis.at(j)->controlWord,
//                   rob->axis.at(j)->errorCode,
//                   rob->axis.at(j)->encoderValue,
//                   rob->axis.at(j)->actPosition,
//                   rob->axis.at(j)->homeOffset,
//                   rob->axis.at(j)->targetPosition,
//                   robCtl->vrepCtl->motorSmiulationMode,
//                   robCtl->vrepCtl->ioSmiulationMode
//                   );
//        }
//        for( int j=0;j<rob->ioModule.size();j++ )
//        {
//            printf("%d:di=%d,do=%d,ai=%d,ao=%d \n",
//                   j,
//                   rob->ioModule.at(j)->diCount,
//                   rob->ioModule.at(j)->doCount,
//                   rob->ioModule.at(j)->aiCount,
//                   rob->ioModule.at(j)->aoCount
//                   );
//        }
//    }
//#endif

}

void EcatMasterVrep::updateDomainData()
{
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob ){
            continue;
        }
        if( !rob->isLinkOk )
            continue;

        if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
        {

           printf("ecatvrep ERROR, robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP");

        }
        else
        {
            //read vrep motor
            VrepObject* vob=robCtl->vobMap.value( rob->robotInfo.robotId, NULL );
            if( vob && p_powerManger!=NULL)
            {

                    for( int i=0;i<rob->ethercatAxis.size();i++ )
                    {
                        if(0)
                        {
                            //需要正确设置轴的类型，才能正确转换。
                            qDebug()<<"vrep feedbackPosition i"<<i<<"degree"<<180.0*vob->getPos[i]/M_PI;
                        }
                        rob->ethercatAxis.at(i)->logicMotors[0]->actPosition = p_powerManger->getVrepRadianToEncoder(
                                                                    rob->robotInfo.robotId, i, vob->getPos[i] );
                        rob->ethercatAxis.at(i)->logicMotors[0]->encoderValue = rob->ethercatAxis.at(i)->logicMotors[0]->actPosition
                                            -rob->ethercatAxis.at(i)->logicMotors[0]->homeOffset;
                        //read state
                        if( (rob->ethercatAxis.at(i)->logicMotors[0]->controlWord & 0x0F) == 0x0b ){
                            rob->ethercatAxis.at(i)->logicMotors[0]->stateWord= 561 ;
                        }else if( (rob->ethercatAxis.at(i)->logicMotors[0]->controlWord & 0x0F) == 0x0F ) {
                            rob->ethercatAxis.at(i)->logicMotors[0]->stateWord = 4663 ;
                        }else  if( (rob->ethercatAxis.at(i)->logicMotors[0]->controlWord & 0x07) == 0x07 ){
                            rob->ethercatAxis.at(i)->logicMotors[0]->stateWord= 0x0023 ;
                        }else  {
                            rob->ethercatAxis.at(i)->logicMotors[0]->stateWord= 561 ;
                        }
                    }
            }
        }
        if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
        {
            printf("ecatvrep ERROR, robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP");

        }
        else
        {
            //read vrep io
        }

        if(1)
        {
            //        if(check_error_delay>0)
            //            check_error_delay--;
            //        else
            rob->test_demo();
            //rob->checkHomeOffset();

//            rob->updateEthercatIoData();
            setServoData(rob);

            for( int i=0;i<rob->ethercatAxis.size();i++ )
            {
                rob->ethercatAxis.at(i)->updateMotorState();
            }

#if D_WRITE_SERVO_DATA_TO_FILE
            std::vector<int> record;
            record.resize( rob->axis.size() );
#endif

            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )

            {
              printf("ecatvrep ERROR, robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP");
            }

#if D_WRITE_SERVO_DATA_TO_FILE
            for( int i=0;i<rob->axis.size();i++ ){
                record[i]=rob->axis.at(i)->targetPosition;
            }
            static qint64 recordDataForAnalysize=-1;
            if(  1 ){
                recordDataForAnalysize++;
                dataAnalysizer->recordDataForAnalysize( record );
            }else{
                if( recordDataForAnalysize>20000  ){
                    recordDataForAnalysize=0;
                    dataAnalysizer->writeDataQueueToFile( D_TMP_SERVO_DATA_FILE );
                }
            }
#endif


            if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
            {

                printf("ecatvrep ERROR, robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP");
            }

            if( robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_REAL )
            {
                if( rob->get_data_flag )
                {
                    if(false==robCtl->vobMap.contains(rob->robotInfo.robotId))
                    {
                        qDebug()<<"error,false==robCtl->vobMap.contains(rob->robotInfo.robotId)";
                        return ;
                    }
                    //write vrep motor
                    VrepObject* vob=robCtl->vobMap.value( rob->robotInfo.robotId, NULL );
                    if( p_powerManger )
                    {

                        for( int i=0;i<rob->ethercatAxis.size();i++ )
                        {

                            vob->setPos[i] = p_powerManger->getVrepEncoderToRadian( rob->robotInfo.robotId,i , 
                                      rob->ethercatAxis.at(i)->logicMotors[0]->targetPosition
                                    + rob->ethercatAxis.at(i)->logicMotors[0]->homeOffset );
                            if(0)
                            {
                                //需要正确设置轴的类型，才能正确转换。
                                qDebug()<<"vrep commandPosition i"<<i<<"degree"<<180.0*vob->setPos[i]/M_PI;
                            }
                        }
                        vob->setChanged=true;
                    }
                }
            }
            if( robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_REAL )
            {
                //write vrep io
            }

        }
    }

#if 0
    static int tCount=0;
    tCount++;
    if(tCount>10000000/cycle_us){
        tCount=0;

        //printNowTime();

        qDebug()<<"========================================";
        for( int j=0;j<rob->axis.size();j++ )
        {
            printf("%d:MD:%d,ST:%d, ctl:%d,err:%d, encode:%d,%d,%d,%d,%d,%d,%d, %d\n",
                   j,
                   rob->axis.at(j)->modeOfOperation,
                   rob->axis.at(j)->stateWord,
                   rob->axis.at(j)->controlWord,
                   rob->axis.at(j)->errorCode,
                   rob->axis.at(j)->encoderValue,
                   rob->axis.at(j)->actPosition,
                   rob->axis.at(j)->homeOffset,
                   rob->axis.at(j)->targetPosition,
                   isSlavesOp,
                   robCtl->vrepCtl->motorSmiulationMode,
                   rob->axis.at(j)->actVelocity,
                   rob->axis.at(j)->targetVelocity
                   );
        }
        for( int j=0;j<rob->ioModule.size();j++ )
        {
            printf("%d:di=%d,do=%d,ai=%d,ao=%d \n",
                   j,
                   rob->ioModule.at(j)->diCount,
                   rob->ioModule.at(j)->doCount,
                   rob->ioModule.at(j)->aiCount,
                   rob->ioModule.at(j)->aoCount
                   );
        }
    }
#endif
}

void EcatMasterVrep::addMsg(int messageLevel,
                            std::string componentName,
                            std::string messageType,
                            int messageCode,
                            int robotId,
                            int parameter1 ,
                            int parameter2 ,
                            int parameter3 ,
                            int parameter4 )
{
//    struct timeval tv_begin ;
//    gettimeofday(&tv_begin, NULL);

    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
//    tmpMsg.time = tv_begin.tv_sec;

    QString infomationStr;
    switch(messageCode)
    {

    case 1002:
    {
        infomationStr =  QObject::tr("系统初始化完成，请您操作(vrep) !");
        break;

    }
    case 301:
    {
        infomationStr = QObject::tr( "机器人轴别名(%1)busDev.xml中node(%2)与实际物理node不一致，请检查物理接线或配置")
                .arg(parameter1).arg(parameter2);
    }
        break;
    case 300:
    {
        infomationStr = QObject::tr( "机器人轴别名%1在总线配置中没有找到!").arg(parameter1) ;
    }
        break;
    case 302:
        break;
    }
    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);

}

